#include <stdio.h>
#include <AntiCollision.h>
#include <float.h>
#include <math.h>
#include <conio.h>

long position;
long gamma;
long theta;
long state;
#include <string.h>
#include "PlcClient.h"

double Safety_distance[9] = { 0, 5, 5, 5, 5, 5, 5, 5, 4 };//When Collision_flag = i, Safety_distance[i] is proposed.
double Vertical_safety_distance = 1.5;

void main()
{
	Stacker_reclaimer_t  Stacker_reclaimer[Stacker_num] = { 0 };
	int init_flag;
	int i;

	TcpInfo_t Tcpinfo[3];   // 0:Reclaimer on track 0,1:Stacker reclaimer on track 1; 2:Stacker reclaimer on track 0;
	Tcpinfo[0].ip = "10.160.48.38";
	Tcpinfo[1].ip = "10.160.48.34";
	Tcpinfo[2].ip = "10.160.48.36";
	Tcpinfo[0].port = Tcpinfo[1].port = Tcpinfo[2].port = 502;
	Tcpinfo[0].readAddr = Tcpinfo[1].readAddr = Tcpinfo[2].readAddr = 950;
	Tcpinfo[0].readLength = Tcpinfo[1].readLength = Tcpinfo[2].readLength = 12;

	double Center_position[3] = { 0 };
	double Rotation_angle[3] = { 0 };
	double Pitch_angle[3] = { 0 };
	long State[3] = { 0 };

	//	double Center_position[3]	=	{ 78.23,210.33,280.76 };   //0~320
	//	double Center_position[3]   =   { 120.23,189.33,238.76 };

	//	double Center_position[3] = {156.35, 89.88, 250.12};
	//	double Center_position[3] = { 109.35, 270.88, 183.12 };
	//	double Center_position[3] = { 170.35, 99.88, 270.12 };
	//	double Rotation_angle[3]	=	{-26.34/180 *PI, -5.12/180 *PI, -77.35/180 *PI };   //-110~110
	//	double Rotation_angle[3] = { -5.34 / 180 * PI, 5.12 / 180 * PI, 70.35 / 180 * PI };   //-110~110
	//	double Pitch_angle[3]		=	{5.42/180 *PI, -2.33/180 *PI, 3.56/180 *PI};		//-15~15
	double Collision_matrix[3][3] = { 0 };
	int c = 0;
	while (c != 'q')
	{
		if (PLC_info_read(Center_position, Rotation_angle, Pitch_angle, State, Tcpinfo) > 0)
		{
		}
		for (i = 0; i < 3; i++)
		{
			init_flag = Stacker_init(Center_position[i], Rotation_angle[i], Pitch_angle[i], &Stacker_reclaimer[i], i);
			if (init_flag < 0)
			{
				printf("\n Stacker %d init error", i);
				break;
			}
			else
			{
				printf("\n Stacker %d init successful", i);
			}
		}
		if (init_flag > 0)
		{
			Anti_collision_detection(Stacker_reclaimer);
			Dangerous_distance_broadcast(Stacker_reclaimer, 3, Collision_matrix);
		}
		printf("\nCenterposition0 before = %f", Center_position[0]);
		printf("\nCenterposition1 before = %f", Center_position[1]);
		printf("\nCenterposition2 before = %f", Center_position[2]);
		//
		//		for (i = 0; i < 3; i++)
		//		{
		//			for (j = 0; j < 3; j++)
		//			{
		//				if ((j == i)|(abs(i - j) == 2)) continue;		//Center_position[0] < Center_position[1] < Center_position[2]
		////				if( (j==i) |((i == 1)& (j == 2))| ((i == 2) & (j == 1))) continue;	//Center_position[1] < Center_position[0] < Center_position[2]
		////				if ((j == i) | ((i == 0) & (j == 1)) | ((i == 1) & (j == 0))) continue;	//Center_position[0] < Center_position[2] < Center_position[1]
		//				else
		//				{
		//					if ((Collision_matrix[i][j] < 8) & (Collision_matrix[i][j] > 0.1))
		//					{
		//						printf("\n Stacker %d and %d are at risk of collision", i, j);
		//						if (Stacker_reclaimer[i].Center_position > Stacker_reclaimer[j].Center_position)
		//						{
		//							Center_position[i] += 0.5;
		//							Center_position[j] -= 0.5;
		//						}
		//						else
		//						{
		//							Center_position[i] -= 0.5;
		//							Center_position[j] += 0.5;
		//						}
		//					}
		//					else
		//					{
		//						if (Stacker_reclaimer[i].Center_position > Stacker_reclaimer[j].Center_position)
		//						{
		//							Center_position[i] -= 0.5;
		//							Center_position[j] += 0.5;
		//						}
		//						else
		//						{
		//							Center_position[i] += 0.5;
		//							Center_position[j] -= 0.5;
		//						}
		//					}
		//				}
		//			}
		//		}
		//		printf("\nCenterposition0 after = %f", Center_position[0]);
		//		printf("\nCenterposition1 after = %f", Center_position[1]);
		//		printf("\nCenterposition2 after = %f", Center_position[2]);
		c = _getch();
	}
	printf("\n Test over~");
}

int PLC_info_read(double* Center_position, double* Rotation_angle, double* Pitch_angle, long* State, TcpInfo_t* Tcpinfo)
{
	PlcClient PLC_Stacker0(Tcpinfo[0].ip, Tcpinfo[0].port);
	PlcClient PLC_Stacker1(Tcpinfo[1].ip, Tcpinfo[1].port);
	PlcClient PLC_Stacker2(Tcpinfo[2].ip, Tcpinfo[2].port);
	PLC_Stacker0.read_run(Tcpinfo[0]);
	Center_position[0] = position / 1000.0;
	Pitch_angle[0] = gamma / 180000.0 * PI;
	Rotation_angle[0] = -theta / 180000.0 * PI;
	State[0] = state;

	PLC_Stacker1.read_run(Tcpinfo[1]);
	Center_position[2] = position / 1000.0;
	Pitch_angle[2] = gamma / 180000.0 * PI;
	Rotation_angle[2] = -theta / 180000.0 * PI;
	State[2] = state;

	PLC_Stacker2.read_run(Tcpinfo[2]);
	Center_position[1] = position / 1000.0;
	Pitch_angle[1] = gamma / 180000.0 * PI;
	Rotation_angle[1] = -theta / 180000.0 * PI;
	State[1] = state;
	if (((Rotation_angle[0] * PI) < 110) & ((Rotation_angle[1] * PI) > -110))
	{
		printf("\nCenter_position = %f, Rotation_angle = %f, Pitch_angle = %f, state = %d\n", Center_position[0], -(Rotation_angle[0] * 57.28), (Pitch_angle[0] * 57.28), State[0]);
		printf("Center_position = %f, Rotation_angle = %f, Pitch_angle = %f, state = %d\n", Center_position[1], -(Rotation_angle[1] * 57.28), (Pitch_angle[1] * 57.28), State[1]);
		printf("Center_position = %f, Rotation_angle = %f, Pitch_angle = %f, state = %d\n", Center_position[2], -(Rotation_angle[2] * 57.28), (Pitch_angle[2] * 57.28), State[2]);
		return 1;
	}
	else return -1;
}

int Stacker_init(double Center_position, double Rotation_angle, double Pitch_angle, Stacker_reclaimer_t* Stacker, int Stacker_No)
{
	double forearm_length = Forearm_length / 1000.0;
	double track_spacing = Track_spacing / 1000.0;
	double counterweight_length = Counterweight_length / 1000.0;
	double track_length = Track_length / 1000.0;
	double belt_projected_length = Belt_projected_length / 1000.0;
	double tail_projected_length = Tail_projected_length / 1000.0;

	Stacker->Center_position = Center_position;
	Stacker->Rotation_angle = Rotation_angle;
	Stacker->Pitch_angle = Pitch_angle;

	//Initialize when the input parameters are correct
	if ((Center_position > tail_projected_length) & (Center_position < track_length) & (Rotation_angle < 110.0) & (Rotation_angle > -110.0))
	{
		//Stacker_No = 0 means reclaimer machine on track 0
		if (Stacker_No == 0)
		{
			Stacker->coor_A[0] = Stacker->Center_position + forearm_length * cos(Stacker->Rotation_angle); //X-axis coor of point A
			Stacker->coor_A[1] = forearm_length * sin(Stacker->Rotation_angle);							//Y-axis coor of point A
			Stacker->coor_B[0] = Stacker->Center_position - counterweight_length * cos(Stacker->Rotation_angle); //X-axis coor of point B
			Stacker->coor_B[1] = 0 - counterweight_length * sin(Stacker->Rotation_angle);                 //Y-axis coor of point B
			Stacker->track_number = 0;
			Stacker->coor_C[0] = Stacker->Center_position - tail_projected_length;
			Stacker->coor_C[1] = 0;
			Stacker->coor_D[0] = Stacker->Center_position;
			Stacker->coor_D[1] = 0;
		}
		//Stacker_No = 1 means Stacker reclaimer on track 1
		else if (Stacker_No == 1)
		{
			Stacker->coor_A[0] = Stacker->Center_position + forearm_length * cos(Stacker->Rotation_angle); //X-axis coor of point A
			Stacker->coor_A[1] = forearm_length * sin(Stacker->Rotation_angle) - track_spacing;			//Y-axis coor of point A
			Stacker->coor_B[0] = Stacker->Center_position - counterweight_length * cos(Stacker->Rotation_angle); //X-axis coor of point B
			Stacker->coor_B[1] = -track_spacing - counterweight_length * sin(Stacker->Rotation_angle);                 //Y-axis coor of point B
			Stacker->coor_C[0] = Stacker->Center_position - belt_projected_length;
			Stacker->coor_C[1] = -track_spacing;
			Stacker->coor_D[0] = Stacker->Center_position;
			Stacker->coor_D[1] = -track_spacing;
			Stacker->track_number = 1;
		}
		//Stacker_No = 2 means Stacker reclaimer on track 0
		else
		{
			Stacker->coor_A[0] = Stacker->Center_position + forearm_length * cos(Stacker->Rotation_angle); //X-axis coor of point A
			Stacker->coor_A[1] = forearm_length * sin(Stacker->Rotation_angle);							//Y-axis coor of point A
			Stacker->coor_B[0] = Stacker->Center_position - counterweight_length * cos(Stacker->Rotation_angle); //X-axis coor of point B
			Stacker->coor_B[1] = 0 - counterweight_length * sin(Stacker->Rotation_angle);                 //Y-axis coor of point B
			Stacker->track_number = 0;
			Stacker->coor_C[0] = Stacker->Center_position - belt_projected_length;
			Stacker->coor_C[1] = 0;
			Stacker->coor_D[0] = Stacker->Center_position;
			Stacker->coor_D[1] = 0;
		}
		Stacker->collision_flag[0] = Stacker->collision_flag[1] = Stacker->collision_flag[2] = 0;
		Stacker->Collision_distance[0] = Stacker->Collision_distance[1] = Stacker->Collision_distance[2] = 0;
		Stacker->alert_level[0] = Stacker->alert_level[1] = Stacker->alert_level[2] = 0;
		Stacker->Vertex[0] = Stacker->Vertex[1] = Stacker->Vertex[2] = 0;
		Stacker->Vertical_flag = 0;
		return 1;
	}
	else
		return -1;
}

// The key is to calculate the distance between characteristic points and edges
void Anti_collision_detection(Stacker_reclaimer_t* Stacker)
{
	double Distance0_1, Distance1_2, Distance0_2, Safety_distance1, Safety_distance2, Safety_distance3, Safety_distance4;
	double forearm_length = Forearm_length / 1000.0;
	double track_spacing = Track_spacing / 1000.0;
	double counterweight_length = Counterweight_length / 1000.0;
	double track_length = Track_length / 1000.0;
	double belt_projected_length = Belt_projected_length / 1000.0;
	double tail_projected_length = Tail_projected_length / 1000.0;
	double radius_bucket_wheel = Radius_bucket_wheel / 1000.0;
	double radius_counterweight = Radius_counterweight / 1000.0;

	Distance0_1 = Distance_calculation(Stacker, 0, 1);
	Distance0_2 = Distance_calculation(Stacker, 0, 2);
	Distance1_2 = Distance_calculation(Stacker, 1, 2);

	//Safety_distance1 indicates the safe distance between stacker reclaimers on different tracks
	Safety_distance1 = sqrt(pow(track_spacing, 2) + \
		pow(sqrt(pow(forearm_length + radius_bucket_wheel, 2) - pow(track_spacing, 2)) + belt_projected_length, 2)) + Safety_threshold;

	//Safety_distance2 indicates the safe distance between stacker reclaimer and reclaimer on the same track
	Safety_distance2 = forearm_length + belt_projected_length + Safety_threshold;

	//Safety_distance3 indicates the safe distance between stacker and the reclaimer on different tracks(X_stacker > X_reclaimer)
	Safety_distance3 = Safety_distance1;

	//Safety_distance3 indicates the safe distance between stacker and the reclaimer on different tracks(X_stacker < X_reclaimer)
	Safety_distance4 = radius_bucket_wheel + radius_counterweight + forearm_length + counterweight_length + Safety_threshold;

	if (Distance1_2 > Safety_distance1)
	{
		printf("\n Distance1_2 = %f, larger than Safety_distance1 = %f", Distance1_2, Safety_distance1);
		printf("\n The situation is safe between the two stackers on different tracks");
		Stacker[1].collision_flag[2] = Stacker[2].collision_flag[1] = 0;
		Stacker[1].Collision_distance[2] = Stacker[2].Collision_distance[1] = Distance1_2;
	}
	else
	{
		printf("\n Collision may happen between the two stackers on different tracks");
		Collision_detected(Stacker, 2, 1);
	}

	if (Distance0_2 > Safety_distance2)
	{
		printf("\n Distance0_2 = %f, larger than Safety_distance2 = %f", Distance0_2, Safety_distance2);
		printf("\n The situation is safe between stacker and relaimer on the same track");
		Stacker[0].collision_flag[2] = Stacker[2].collision_flag[0] = 0;
		Stacker[0].Collision_distance[2] = Stacker[2].Collision_distance[0] = Distance0_2;
	}
	else
	{
		printf("\n Collision may happen between the two stackers on different tracks");
		Collision_detected(Stacker, 0, 2);
	}

	if (Distance0_1 > (Stacker[0].Center_position < Stacker[1].Center_position ? Safety_distance3 : Safety_distance4))
	{
		if (Stacker[0].Center_position < Stacker[1].Center_position)
			printf("\n Distance0_1 = %f, larger than Safety_distance3  = %f", Distance0_1, Safety_distance3);
		else
			printf("\n Distance0_1 = %f, larger than Safety_distance4  = %f", Distance0_1, Safety_distance4);
		printf("\n The situation is safe between stacker and relaimer on the different track");
		Stacker[0].collision_flag[1] = Stacker[1].collision_flag[0] = 0;
		Stacker[0].Collision_distance[1] = Stacker[1].Collision_distance[0] = Distance0_1;
	}
	else
	{
		printf("\n Collision may happen between the two stackers on different tracks");
		Collision_detected(Stacker, 0, 1);
	}
}
void Dangerous_distance_broadcast(Stacker_reclaimer_t* Stacker, int num, double Collision_matrix[3][3])
{
	uint8 i, j;
	for (i = 0; i < num; i++)
	{
		for (j = 0; j < num; j++)
		{
			if (j == i) continue;
			else
			{
				switch (Stacker[i].collision_flag[j])
				{
				case 0:
					printf("\n Stacker %d and %d have no collision risk", i, j);
					printf("\n Distance between stacker %d and stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					break;
				case 1:
					printf("\n Point A of Stacker %d may collide with forearm of Stacker %d", i, j);
					printf("\n Distance between point A and forearm/counterweight is %f", Stacker[i].Collision_distance[j]);
					if (Stacker[i].Collision_distance[j] < Safety_distance[1])
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				case 2:
					printf("\n Point B of Stacker %d may collide with forearm of Stacker %d", i, j);
					printf("\n Distance between point B and forearm/counterweigt is %f", Stacker[i].Collision_distance[j]);
					if (Stacker[i].Collision_distance[j] < Safety_distance[2])
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				case 3:
					printf("\n Point C of Stacker %d may collide with forearm of Stacker %d", i, j);
					printf("\n Distance between point C and forearm/counterweight is %f", Stacker[i].Collision_distance[j]);
					if ((Stacker[i].Collision_distance[j] < Safety_distance[3]) & (Stacker[i].Vertical_distance < Vertical_safety_distance))
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				case 4:
					printf("\n Point A of Stacker %d may collide with tail/belt of Stacker %d", i, j);
					printf("\n Distance between point A and belt/tail is %f", Stacker[i].Collision_distance[j]);
					if ((Stacker[i].Collision_distance[j] < Safety_distance[4]) & (Stacker[i].Vertical_distance < Vertical_safety_distance))
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				case 5:
					printf("\n Point B of Stacker %d may collide with tail/belt of Stacker %d", i, j);
					printf("\n Distance between point B and belt/tail is %f", Stacker[i].Collision_distance[j]);
					if ((Stacker[i].Collision_distance[j] < Safety_distance[5]) & (Stacker[i].Vertical_distance < Vertical_safety_distance))
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				case 6:
					printf("\n forearm or counter weight of stacker %d may be impacted by stacker %d", i, j);
					printf("\n Distance between impact point and forearm/counterweight is %f", Stacker[i].Collision_distance[j]);
					if (Stacker[i].Collision_distance[j] < Safety_distance[6])
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				case 7:
					printf("\n belt or tail will be impacted of stacker %d may be impacted by stacker %d", i, j);
					printf("\n Distance between impact point and belt/tail is %f", Stacker[i].Collision_distance[j]);
					if (Stacker[i].Collision_distance[j] < Safety_distance[7])
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				case 8:
					if ((Stacker[i].Vertex[j] == 1) & (Stacker[j].Vertex[i] == 1))
						printf("\n Distance between point A of stacker %d and point A of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					else if ((Stacker[i].Vertex[j] == 1) & (Stacker[j].Vertex[i] == 2))
						printf("\n Distance between point A of stacker %d and point B of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					else if ((Stacker[i].Vertex[j] == 1) & (Stacker[j].Vertex[i] == 3))
						printf("\n Distance between point A of stacker %d and point C of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					else if ((Stacker[i].Vertex[j] == 2) & (Stacker[j].Vertex[i] == 1))
						printf("\n Distance between point B of stacker %d and point A of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					else if ((Stacker[i].Vertex[j] == 3) & (Stacker[j].Vertex[i] == 1))
						printf("\n Distance between point C of stacker %d and point A of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					else if ((Stacker[i].Vertex[j] == 3) & (Stacker[j].Vertex[i] == 2))
						printf("\n Distance between point C of stacker %d and point B of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					else if ((Stacker[i].Vertex[j] == 2) & (Stacker[j].Vertex[i] == 3))
						printf("\n Distance between point B of stacker %d and point C of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					else
						printf("\n Distance between point B of stacker %d and point B of stacker %d is %f", i, j, Stacker[i].Collision_distance[j]);
					if (Stacker[i].Collision_distance[j] < Safety_distance[8])
						printf("\n The actual distance is smaller than safety distance, Stacker %d should be stopped", i);
					break;
				default:
					break;
				}
				Collision_matrix[i][j] = Stacker[i].Collision_distance[j];
			}
		}
		if (Stacker[i].Vertical_flag == 1)
		{
			if (Stacker[i].Vertical_distance < 2)
				printf("\n Stacker %d cannot pass the track, the vertical distance is %f", i, Stacker[i].Vertical_distance);
			else
				printf("\n Stacker %d can pass the track, the vertical distance is %f", i, Stacker[i].Vertical_distance);
		}
	}
}

double Distance_calculation(Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2)
{
	double Distance_X = fabs(Stacker[Stacker_No1].coor_D[0] - Stacker[Stacker_No2].coor_D[0]);
	double Distance_Y = fabs(Stacker[Stacker_No1].coor_D[1] - Stacker[Stacker_No2].coor_D[1]);
	return sqrt(pow(Distance_X, 2) + pow(Distance_Y, 2));
}

//find out which point will be the collision point in what way	
void  Collision_detected(Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2)
{
	double Slope1, Slope2, b1, b2;
	double DoC1_A, DoC1_B, DoC1_C;	//DoC1_A:Distance from characteristic point A of Stacker1 to forearm edge of Stacker2  
									//DoC1_B:Distance from characteristic point B to Stacker1 to forearm edge of Stacker2 
									//DoC1_C:Distance from characteristic point C of Stacker1 to forearm edge of Stacker1

	double DoC2_A, DoC2_B, DoC2_C;	//DoC2_A:Distance from characteristic point A of Stacker2 to forearm edge of Stacker2  
									//DoC2_B:Distance from characteristic point B to Stacker2 to forearm edge of Stacker2 
									//DoC2_C:Distance from characteristic point C of Stacker2 to forearm edge of Stacker1

	double DoC1_inX, DoC2_inX;		//DoC1_inX:Distance from characteristic point A or B of Stacker1 to axis X(Depends on rotation angle)
									//DoC2_inX:Distance from characteristic point A or B of Stacker1 to axis X(Depends on rotation angle)

	double DoC1_inY, DoC2_inY;		//DoC1_inX:Distance from characteristic point A or B of Stacker1 to axis Y(Depends on rotation angle)
									//DoC2_inX:Distance from characteristic point A or B of Stacker1 to axis Y(Depends on rotation angle)
	double coor_pedal[2];
	//Stacker Reclaimer on different tracks
	double DoC[8] = { 0 };
	uint8 num = 0;
	double Current_dangerous_distance;
	// stacker recleaimers on different tracks
	if ((Stacker_No1 == 1) | (Stacker_No2 == 1))
	{
		Slope1 = tan(Stacker[Stacker_No1].Rotation_angle);						//  Stacker on track 0
		Slope2 = tan(Stacker[Stacker_No2].Rotation_angle);						//  Stacker on track 1
		b1 = (-1) * Slope1 * (Stacker[Stacker_No1].Center_position);
		b2 = (-1) * Slope2 * (Stacker[Stacker_No2].Center_position) - 12.0;

		//Distance between characteristic point and edge
		DoC[0] = DoC1_A = Distance_from_point_to_line(Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_A[1], Slope2, b2, \
			Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_B[0]);
		DoC[1] = DoC1_B = Distance_from_point_to_line(Stacker[Stacker_No1].coor_B[0], Stacker[Stacker_No1].coor_B[1], Slope2, b2, \
			Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_B[0]);
		DoC[2] = DoC1_C = Distance_from_point_to_line(Stacker[Stacker_No1].coor_C[0], Stacker[Stacker_No1].coor_C[1], Slope2, b2, \
			Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_B[0]);

		//Distance between characteristic point and edge
		DoC[3] = DoC2_A = Distance_from_point_to_line(Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_A[1], Slope1, b1, \
			Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_B[0]);
		DoC[4] = DoC2_B = Distance_from_point_to_line(Stacker[Stacker_No2].coor_B[0], Stacker[Stacker_No2].coor_B[1], Slope1, b1, \
			Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_B[0]);
		DoC[5] = DoC2_C = Distance_from_point_to_line(Stacker[Stacker_No2].coor_C[0], Stacker[Stacker_No2].coor_C[1], Slope1, b1, \
			Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_B[0]);

		//When belt or tail edge are in danger
		if (Stacker[Stacker_No1].Rotation_angle > 0)
		{
			DoC1_inX = Stacker[Stacker_No1].coor_B[0];
			if ((DoC1_inX < Stacker[Stacker_No2].coor_D[0]) & (DoC1_inX > Stacker[Stacker_No2].coor_C[0]))
			{
				if (Stacker[Stacker_No1].coor_B[1] > -12)
					DoC1_inY = Stacker[Stacker_No1].coor_B[1] + 12;
				else DoC1_inY = -1;
			}
			else DoC1_inY = -1;
		}
		else
		{
			DoC1_inX = Stacker[Stacker_No1].coor_A[0];
			if ((DoC1_inX < Stacker[Stacker_No2].coor_D[0]) & (DoC1_inX > Stacker[Stacker_No2].coor_C[0]))
			{
				if (Stacker[Stacker_No1].coor_A[1] > -12)
					DoC1_inY = Stacker[Stacker_No1].coor_A[1] + 12;
				else DoC1_inY = -1;
			}
			else DoC1_inY = -1;
		}
		DoC[6] = DoC1_inY;
		if (Stacker[Stacker_No2].Rotation_angle > 0)
		{
			DoC2_inX = Stacker[Stacker_No2].coor_A[0];
			if ((DoC2_inX < Stacker[Stacker_No1].coor_D[0]) & (DoC2_inX > Stacker[Stacker_No1].coor_C[0]))
			{
				if (Stacker[Stacker_No2].coor_A[1] < 0)
					DoC2_inY = -Stacker[Stacker_No2].coor_A[1];
				else DoC2_inY = -1;
			}
			else DoC2_inY = -1;
		}
		else
		{
			DoC2_inX = Stacker[Stacker_No2].coor_B[0];
			if ((DoC2_inX < Stacker[Stacker_No1].coor_D[0]) & (DoC2_inX > Stacker[Stacker_No1].coor_C[0]))
			{
				if (Stacker[Stacker_No2].coor_B[1] < 0)
					DoC2_inY = -Stacker[Stacker_No2].coor_B[1];
				else DoC2_inY = -1;
			}
			else DoC2_inY = -1;
		}
		DoC[7] = DoC2_inY;

		//Find the point where collision is most likely to occur
		Current_dangerous_distance = Current_dangerous_distance_calculate(DoC, 8, &num);
		Vertex_distance(Stacker, Stacker_No1, Stacker_No2);
		if (Current_dangerous_distance > Stacker[Stacker_No1].Collision_distance[Stacker_No2])
			return;
		Stacker[Stacker_No1].Collision_distance[Stacker_No2] = Current_dangerous_distance;
		Stacker[Stacker_No2].Collision_distance[Stacker_No1] = Current_dangerous_distance;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = Stacker[Stacker_No2].Vertex[Stacker_No1] = 0;
		switch (num)
		{
		case 0:
			Stacker[Stacker_No1].collision_flag[Stacker_No2] = 1;
			Stacker[Stacker_No2].collision_flag[Stacker_No1] = 6;
			if (Stacker[Stacker_No1].Collision_distance[Stacker_No2] < Safety_distance[0])
			{
				Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
				Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			}
			break;
		case 1:
			Stacker[Stacker_No1].collision_flag[Stacker_No2] = 2;
			Stacker[Stacker_No2].collision_flag[Stacker_No1] = 6;
			if (Stacker[Stacker_No1].Collision_distance[Stacker_No2] < Safety_distance[1])
			{
				Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
				Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			}
			break;
		case 2:
			Stacker[Stacker_No1].collision_flag[Stacker_No2] = 3;
			Stacker[Stacker_No2].collision_flag[Stacker_No1] = 6;
			if (fabs(Stacker[Stacker_No2].Rotation_angle) != 0.5 * PI)
			{
				pedal_coor_calculation(Stacker[Stacker_No1].coor_C[0], Stacker[Stacker_No1].coor_C[1], Slope2, b2, coor_pedal);
			}
			else
				coor_pedal[1] = Stacker[Stacker_No1].coor_C[1];
			if (Stacker[Stacker_No2].Rotation_angle > 0)
			{
				if (coor_pedal[1] > Stacker[Stacker_No2].coor_D[1]) //pedal amoung D-A;
					Vertical_collision_detection(Stacker, Stacker_No2, Stacker_No1);
				else
					Vertical_collision_detection_B(Stacker, Stacker_No2, Stacker_No1);
			}
			else
			{
				if (coor_pedal[1] > Stacker[Stacker_No2].coor_D[1]) //pedal amoung D-B;
					Vertical_collision_detection_B(Stacker, Stacker_No2, Stacker_No1);
				else
					Vertical_collision_detection(Stacker, Stacker_No2, Stacker_No1);
			}
			if (Stacker[Stacker_No1].Collision_distance[Stacker_No2] < Safety_distance[2])
			{
				Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
				Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			}
			break;
		case 3:
			Stacker[Stacker_No1].collision_flag[Stacker_No2] = 6;
			Stacker[Stacker_No2].collision_flag[Stacker_No1] = 1;
			if (Stacker[Stacker_No2].Collision_distance[Stacker_No1] < Safety_distance[0])
			{
				Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
				Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			}
			break;
		case 4:
			Stacker[Stacker_No1].collision_flag[Stacker_No2] = 6;
			Stacker[Stacker_No2].collision_flag[Stacker_No1] = 2;
			if (Stacker[Stacker_No2].Collision_distance[Stacker_No1] < Safety_distance[1])
			{
				Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
				Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			}
			break;
		case 5:
			Stacker[Stacker_No1].collision_flag[Stacker_No2] = 6;
			Stacker[Stacker_No2].collision_flag[Stacker_No1] = 3;
			if (fabs(Stacker[Stacker_No1].Rotation_angle) != 0.5 * PI)
			{
				pedal_coor_calculation(Stacker[Stacker_No2].coor_C[0], Stacker[Stacker_No2].coor_C[1], Slope1, b1, coor_pedal);
			}
			else
				coor_pedal[1] = Stacker[Stacker_No2].coor_C[1];
			if (Stacker[Stacker_No1].Rotation_angle > 0)
			{
				if (coor_pedal[1] > Stacker[Stacker_No1].coor_D[1]) //pedal amoung D-A;
					Vertical_collision_detection(Stacker, Stacker_No1, Stacker_No2);
				else
					Vertical_collision_detection_B(Stacker, Stacker_No1, Stacker_No2);
			}
			else
			{
				if (coor_pedal[1] > Stacker[Stacker_No1].coor_D[1]) //pedal amoung D-B;
					Vertical_collision_detection(Stacker, Stacker_No1, Stacker_No2);
				else
					Vertical_collision_detection_B(Stacker, Stacker_No1, Stacker_No2);
			}
			if (Stacker[Stacker_No2].Collision_distance[Stacker_No1] < Safety_distance[2])
			{
				Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
				Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			}
			break;
		case 6:
			if (Stacker[Stacker_No1].Rotation_angle > 0)
			{
				Stacker[Stacker_No1].collision_flag[Stacker_No2] = 5;
				Vertical_collision_detection_B(Stacker, Stacker_No1, Stacker_No2);
				if (Stacker[Stacker_No1].Collision_distance[Stacker_No2] < Safety_distance[4])
				{
					Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
					Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
				}
			}
			else
			{
				Stacker[Stacker_No1].collision_flag[Stacker_No2] = 4;
				Vertical_collision_detection(Stacker, Stacker_No1, Stacker_No2);
				if (Stacker[Stacker_No1].Collision_distance[Stacker_No2] < Safety_distance[3])
				{
					Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
					Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
				}
			}
			Stacker[Stacker_No2].collision_flag[Stacker_No1] = 7;
			break;
		case 7:
			if (Stacker[Stacker_No2].Rotation_angle > 0)
			{
				Stacker[Stacker_No2].collision_flag[Stacker_No1] = 4;
				Vertical_collision_detection(Stacker, Stacker_No2, Stacker_No1);
				if (Stacker[Stacker_No2].Collision_distance[Stacker_No1] < Safety_distance[3])
				{
					Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
					Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
				}
			}
			else
			{
				Stacker[Stacker_No2].collision_flag[Stacker_No1] = 5;
				Vertical_collision_detection_B(Stacker, Stacker_No2, Stacker_No1);
				if (Stacker[Stacker_No2].Collision_distance[Stacker_No1] < Safety_distance[4])
				{
					Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
					Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
				}
			}
			Stacker[Stacker_No1].collision_flag[Stacker_No2] = 7;
			break;
		default:	break;
		}
	}
	// on the same track stacker[0].coor_D[0] < stacker[2].coor_D[0]
	// 1:point A of Stacker[0] 2: point B of Stacker[2]
	else
	{
		Slope1 = tan(Stacker[Stacker_No1].Rotation_angle);						//  Stacker on track 0
		Slope2 = tan(Stacker[Stacker_No2].Rotation_angle);						//  Stacker on track 1
		b1 = (-1) * Slope1 * (Stacker[Stacker_No1].Center_position);
		b2 = (-1) * Slope2 * (Stacker[Stacker_No2].Center_position);

		DoC[0] = DoC1_A = Distance_from_point_to_line(Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_A[1], Slope2, b2, \
			Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_B[0]);
		DoC[1] = DoC2_B = Distance_from_point_to_line(Stacker[Stacker_No2].coor_B[0], Stacker[Stacker_No2].coor_B[1], Slope1, b1, \
			Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_B[0]);
		DoC[2] = DoC2_C = Distance_from_point_to_line(Stacker[Stacker_No2].coor_C[0], Stacker[Stacker_No2].coor_C[1], Slope1, b1, \
			Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_B[0]);
		if (fabs(Stacker[Stacker_No1].Rotation_angle) > 0.175)				//|Rotation angle| > 10
		{
			Current_dangerous_distance = Current_dangerous_distance_calculate(DoC, 3, &num);
			Vertex_distance(Stacker, Stacker_No1, Stacker_No2);
			if (Current_dangerous_distance > Stacker[Stacker_No1].Collision_distance[Stacker_No2])
				return;
			Stacker[Stacker_No1].Collision_distance[Stacker_No2] = Stacker[Stacker_No2].Collision_distance[Stacker_No1] = Current_dangerous_distance;
			switch (num)
			{
			case 0:
				Stacker[Stacker_No1].collision_flag[Stacker_No2] = 1;
				Stacker[Stacker_No2].collision_flag[Stacker_No1] = 6;
				if (Stacker[Stacker_No1].Collision_distance[Stacker_No2] < Safety_distance[0])
				{
					Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
					Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
				}
				break;
			case 1:
				Stacker[Stacker_No1].collision_flag[Stacker_No2] = 6;
				Stacker[Stacker_No2].collision_flag[Stacker_No1] = 2;
				if (Stacker[Stacker_No2].Collision_distance[Stacker_No1] < Safety_distance[1])
				{
					Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
					Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
				}
				break;
			case 2:
				Stacker[Stacker_No1].collision_flag[Stacker_No2] = 6;
				Stacker[Stacker_No2].collision_flag[Stacker_No1] = 3;
				Vertical_collision_detection(Stacker, Stacker_No1, Stacker_No2);
				if (Stacker[Stacker_No2].Collision_distance[Stacker_No1] < Safety_distance[2])
				{
					Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
					Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
				}
				break;
			default:
				break;
			}
		}
		else
		{
			Vertical_collision_detection(Stacker, Stacker_No1, Stacker_No2);
			if (Stacker[Stacker_No1].Vertical_flag == 1)
			{
				Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
				Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			}
		}
		if (Stacker[Stacker_No2].coor_C[0] - Stacker[Stacker_No1].coor_D[0] < 12)
		{
			Stacker[Stacker_No1].alert_level[Stacker_No2] = 2;
			Stacker[Stacker_No2].alert_level[Stacker_No1] = 2;
			Stacker[Stacker_No1].shift[0] = 0;
			Stacker[Stacker_No2].shift[1] = 0;
		}
	}
	if (Stacker[Stacker_No1].alert_level[Stacker_No2] == 2)
	{
		collision_reaction(Stacker, Stacker_No1, Stacker_No2);
	}
	if (Stacker[Stacker_No2].Rotation_angle > 0)
	{
		if ((DoC[7] < Safety_distance[3]) & (DoC[7] > 0))
		{
			Vertical_collision_detection(Stacker, Stacker_No2, Stacker_No1);
			if (Stacker[Stacker_No2].Vertical_distance < Vertical_safety_distance)
				Stacker[Stacker_No2].Rotation[1] = 0;
			else
				Stacker[Stacker_No2].Pitch[1] = 0;
		}
	}
	else
	{
		if ((DoC[7] < Safety_distance[4]) & (DoC[7] > 0))
		{
			Vertical_collision_detection_B(Stacker, Stacker_No2, Stacker_No1);
			if (Stacker[Stacker_No2].Vertical_distance < Vertical_safety_distance)
			{
				if (Stacker[Stacker_No2].Vertical_distance < Vertical_safety_distance)
					Stacker[Stacker_No2].Rotation[0] = 0;
				else
					Stacker[Stacker_No2].Pitch[0] = 0;
			}
		}
	}

	if (Stacker[Stacker_No1].Rotation_angle > 0)
	{
		if ((DoC[6] < Safety_distance[4]) & (DoC[6] > 0))
		{
			Vertical_collision_detection_B(Stacker, Stacker_No1, Stacker_No2);
			if (Stacker[Stacker_No1].Vertical_distance < Vertical_safety_distance)
				Stacker[Stacker_No1].Rotation[1] = 0;
			else
				Stacker[Stacker_No1].Pitch[0] = 0;
		}
	}
	else
	{
		if ((DoC[6] < Safety_distance[3]) & (DoC[6] > 0))
		{
			Vertical_collision_detection(Stacker, Stacker_No1, Stacker_No2);
			if (Stacker[Stacker_No1].Vertical_distance < Vertical_safety_distance)
				Stacker[Stacker_No1].Rotation[0] = 0;
			else
				Stacker[Stacker_No1].Pitch[1] = 0;
		}
	}
}
//Stacker[Stacker_No1].Center_position < Stacker[Stacker_No2].Center_position. Record Stacker_No1's collision distance;

void Vertical_collision_detection(Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2)
{
	double bucket_height;
	if (Stacker_No1 == 0)
		bucket_height = Bucket_height_reclaimer / 1000.0;
	else
		bucket_height = Bucket_height_stacker / 1000.0;
	if (Stacker_No2 == 0)   //Reclaimer may collisde by stacker1
	{
		printf("\nThe reclaimer does not allow space collision between stacker %d and reclaimer %d", Stacker_No1, Stacker_No2);
	}
	else if ((Stacker_No1 == 0) && (Stacker_No2 == 2))
	{
		if (Stacker[Stacker_No2].coor_C[0] - Stacker[Stacker_No1].Center_position < 33)  // < forearm length
		{
			Stacker[Stacker_No1].Vertical_distance = bucket_height + 33 * sin(Stacker[Stacker_No1].Pitch_angle) - \
				(Stacker[Stacker_No1].Center_position + 33 * cos(Stacker[Stacker_No1].Pitch_angle) - Stacker[Stacker_No2].coor_C[0]) * tan(0.2618) - 2.8 - 1.65 - 1.2;
			Stacker[Stacker_No1].Vertical_flag = 1;
		}
		else
		{
			Stacker[Stacker_No1].Vertical_distance = 100.0;
			printf("\n No vertical collision risk between stacker %d and stacker %d", Stacker_No1, Stacker_No2);
		}
	}
	else
	{
		if (Stacker[Stacker_No2].coor_C[0] - Stacker[Stacker_No1].Center_position < 30.7)
		{
			Stacker[Stacker_No1].Vertical_distance = bucket_height + 33 * sin(Stacker[Stacker_No1].Pitch_angle) - \
				(Stacker[Stacker_No1].Center_position + 30.7 - Stacker[Stacker_No2].coor_C[0]) * tan(0.2618) - 2.8 - 1.65 - 1.2;
			Stacker[Stacker_No1].Vertical_flag = 1;
		}
		else
		{
			Stacker[Stacker_No1].Vertical_distance = 100.0;
			printf("\n No vertical collision risk between stacker %d and stacker %d", Stacker_No1, Stacker_No2);
		}
	}
}

void Vertical_collision_detection_B(Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2)		//Stacker_No1 collide, Stacker_No2 be collided
{
	double counter_height;
	if (Stacker_No1 == 0)
		counter_height = counterweight_height_relaimer;
	else
		counter_height = counterweight_height_stacker;
	if (Stacker_No2 == 0)   //Reclaimer may collisde by stacker1
	{
		printf("\nThe reclaimer does not allow space collision between stacker %d and reclaimer %d", Stacker_No1, Stacker_No2);
	}
	else
	{
		if (Stacker[Stacker_No1].Center_position - 12 > Stacker[Stacker_No2].coor_C[0])
		{
			Stacker[Stacker_No1].Vertical_distance = counter_height - 17.0 * sin(Stacker[Stacker_No1].Pitch_angle) - (Stacker[Stacker_No1].coor_D[0] - 12 - Stacker[Stacker_No2].coor_C[0]) * tan(0.2618) - 1 - 1.65 - 1.2;
			Stacker[Stacker_No1].Vertical_flag = 1;
		}
		else
		{
			Stacker[Stacker_No1].Vertical_distance = 100;
			Stacker[Stacker_No1].Vertical_flag = 0;
		}
	}
}
//From point (X,Y) to line Y = kX+b;
double Distance_from_point_to_line(double coor_X, double coor_Y, double k, double b, double A, double B)
{
	double Abs, Sqrt, P_X0, Divisor, Dividend, Distance;  //P_X0: X coordinate of projection point
	double Max_X, Min_X;
	Abs = fabs(k * coor_X - coor_Y + b);	//|kX-Y+b|
	Sqrt = sqrt(k * k + 1);
	Distance = Abs / Sqrt;
	//Verify whether the projection point is between points A and B
	Dividend = k * (coor_Y - b) + coor_X;
	Divisor = k * k + 1;
	P_X0 = Dividend / Divisor;
	if (A > B)
	{
		Max_X = A;
		Min_X = B;
	}
	else
	{
		Max_X = B;
		Min_X = A;
	}
	if ((P_X0 > Min_X) && (P_X0 < Max_X))
		return Distance;
	else
		return -1.0;
}


double Current_dangerous_distance_calculate(double* DoC, uint8 Num, uint8* n)
{
	double Smallest_DoC;
	*n = 8;   //None of the Distance matches
//	Width_forearm = Forearm_width / 1000.0;
	if (*DoC > 0)
	{
		Smallest_DoC = *DoC;
		*n = 0;
	}
	else
		Smallest_DoC = 100;   //A larger value
	int i, j = 0;
	for (i = 1; i < Num; i++)
	{
		DoC++;
		if ((Smallest_DoC > *DoC) & (*DoC > 0))
		{
			Smallest_DoC = *DoC;
			*n = i;
		}
	}
	return Smallest_DoC;
}

void Vertex_distance(Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2)
{
	double A1_A2, A1_B2, B1_A2, B1_B2, DeltaX, DeltaY;
	double A1_C2, B1_C2, C1_A2, C1_B2;
	double Smallest_distance;
	DeltaX = fabs(Stacker[Stacker_No1].coor_A[0] - Stacker[Stacker_No2].coor_A[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_A[1] - Stacker[Stacker_No2].coor_A[1]);
	A1_A2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	DeltaX = fabs(Stacker[Stacker_No1].coor_A[0] - Stacker[Stacker_No2].coor_B[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_A[1] - Stacker[Stacker_No2].coor_B[1]);
	A1_B2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	DeltaX = fabs(Stacker[Stacker_No1].coor_B[0] - Stacker[Stacker_No2].coor_A[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_B[1] - Stacker[Stacker_No2].coor_A[1]);
	B1_A2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	DeltaX = fabs(Stacker[Stacker_No1].coor_B[0] - Stacker[Stacker_No2].coor_B[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_B[1] - Stacker[Stacker_No2].coor_B[1]);
	B1_B2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	DeltaX = fabs(Stacker[Stacker_No1].coor_A[0] - Stacker[Stacker_No2].coor_C[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_A[1] - Stacker[Stacker_No2].coor_C[1]);
	A1_C2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	DeltaX = fabs(Stacker[Stacker_No1].coor_B[0] - Stacker[Stacker_No2].coor_C[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_B[1] - Stacker[Stacker_No2].coor_C[1]);
	B1_C2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	DeltaX = fabs(Stacker[Stacker_No1].coor_C[0] - Stacker[Stacker_No2].coor_A[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_C[1] - Stacker[Stacker_No2].coor_A[1]);
	C1_A2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	DeltaX = fabs(Stacker[Stacker_No1].coor_C[0] - Stacker[Stacker_No2].coor_B[0]);
	DeltaY = fabs(Stacker[Stacker_No1].coor_C[1] - Stacker[Stacker_No2].coor_B[1]);
	C1_B2 = sqrt(pow(DeltaX, 2) + pow(DeltaY, 2));

	Smallest_distance = A1_A2;
	Stacker[Stacker_No1].Vertex[Stacker_No2] = Stacker[Stacker_No2].Vertex[Stacker_No1] = 1;
	if (Smallest_distance > A1_B2)
	{
		Smallest_distance = A1_B2;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = 1;
		Stacker[Stacker_No2].Vertex[Stacker_No1] = 2;
	}
	if (Smallest_distance > B1_A2)
	{
		Smallest_distance = B1_A2;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = 2;
		Stacker[Stacker_No2].Vertex[Stacker_No1] = 1;
	}
	if (Smallest_distance > B1_B2)
	{
		Smallest_distance = B1_B2;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = 2;
		Stacker[Stacker_No2].Vertex[Stacker_No1] = 2;
	}
	if (Smallest_distance > C1_A2)
	{
		Smallest_distance = C1_A2;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = 3;
		Stacker[Stacker_No2].Vertex[Stacker_No1] = 1;
	}

	if (Smallest_distance > C1_B2)
	{
		Smallest_distance = C1_B2;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = 3;
		Stacker[Stacker_No2].Vertex[Stacker_No1] = 2;
	}

	if (Smallest_distance > A1_C2)
	{
		Smallest_distance = A1_C2;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = 1;
		Stacker[Stacker_No2].Vertex[Stacker_No1] = 3;
	}

	if (Smallest_distance > B1_C2)
	{
		Smallest_distance = B1_C2;
		Stacker[Stacker_No1].Vertex[Stacker_No2] = 2;
		Stacker[Stacker_No2].Vertex[Stacker_No1] = 3;
	}
	Stacker[Stacker_No1].collision_flag[Stacker_No2] = Stacker[Stacker_No2].collision_flag[Stacker_No1] = 8;
	Stacker[Stacker_No1].Collision_distance[Stacker_No2] = Stacker[Stacker_No2].Collision_distance[Stacker_No1] = Smallest_distance;
}

void collision_reaction(Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2)
{
	double Slope1, Slope2, b1, b2, Slope_pedal, Slope_collision_point;
	uint8 quadrant;
	double coor_Y;
	if (fabs(Stacker[Stacker_No1].Rotation_angle) != 0.5 * PI)
		Slope1 = tan(Stacker[Stacker_No1].Rotation_angle);						//  Stacker on track 0
	else Slope1 = 10000;
	if (fabs(Stacker[Stacker_No2].Rotation_angle) != 0.5 * PI)
		Slope2 = tan(Stacker[Stacker_No2].Rotation_angle);
	else Slope2 = 10000;						//  Stacker on track 1
	if ((Stacker_No1 == 0) & (Stacker_No2 == 2))							//two stackers on the same track
	{
		b1 = (-1) * Slope1 * (Stacker[Stacker_No1].Center_position);
		b2 = (-1) * Slope2 * (Stacker[Stacker_No2].Center_position);
		if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 1) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 6))
		{
			if (Stacker[Stacker_No2].Rotation_angle > 0)
			{
				Stacker[Stacker_No1].Rotation[0] = 0;
				Stacker[Stacker_No2].Rotation[0] = 0;
			}
			else
			{
				Stacker[Stacker_No1].Rotation[1] = 0;
				Stacker[Stacker_No2].Rotation[1] = 0;
			}
			Stacker[Stacker_No1].shift[0] = 0;
			Stacker[Stacker_No2].shift[1] = 0;
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 6) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 2))		//Stacker_No1's forearm and Stacker_No2's B
		{
			if (Stacker[Stacker_No2].Rotation_angle > 0)
			{
				Stacker[Stacker_No1].Rotation[1] = 0;
				Stacker[Stacker_No2].Rotation[1] = 0;
			}
			else
			{
				Stacker[Stacker_No1].Rotation[0] = 0;
				Stacker[Stacker_No2].Rotation[0] = 0;
			}
			Stacker[Stacker_No1].shift[0] = 0;
			Stacker[Stacker_No2].shift[1] = 0;
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 6) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 3))
		{
			if (Stacker[Stacker_No1].Vertical_distance < Vertical_safety_distance)
			{
				if (Stacker[Stacker_No1].Rotation_angle > 0)
				{
					Stacker[Stacker_No1].Rotation[0] = 0;		//Clockwise disable
				}
				else
				{
					Stacker[Stacker_No1].Rotation[1] = 0;		//Counterclockwise disable
				}
			}
			else
			{
				Stacker[Stacker_No1].Pitch[1] = 0;
				printf("\nStacker %d's point C and stacker %d's forearm", Stacker_No2, Stacker_No1);
			}
			Stacker[Stacker_No1].shift[0] = 0;
			Stacker[Stacker_No2].shift[1] = 0;
		}
		else
		{
			if (Stacker[Stacker_No1].Vertical_distance < Vertical_safety_distance)
			{
				Stacker[Stacker_No1].shift[0] = 0;
				Stacker[Stacker_No2].shift[1] = 0;
			}
			Stacker[Stacker_No1].Pitch[1] = 0;
		}
	}
	else      //Stacker combo: (0,1) , (2,1),(1,0),(1,2)
	{
		if (Stacker_No1 == 1)
		{
			b1 = (-1) * Slope1 * (Stacker[Stacker_No1].Center_position) - 12.0;
			b2 = (-1) * Slope2 * (Stacker[Stacker_No2].Center_position);
		}
		else
		{
			b1 = (-1) * Slope1 * (Stacker[Stacker_No1].Center_position);
			b2 = (-1) * Slope2 * (Stacker[Stacker_No2].Center_position) - 12.0;
		}

		if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 1) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 6))
		{
			vector_angle_calculation(Stacker, Stacker_No1, Stacker_No2, Slope2, b2, Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_A[1]);
			if ((Stacker[Stacker_No1].Rotation_angle != 0) & (Stacker[Stacker_No2].Rotation_angle != 0))
			{
				if (shift_point_calculation(Stacker[Stacker_No2].Rotation_angle, b2, Stacker_No1, Stacker[Stacker_No1].coor_D[0], Stacker[Stacker_No2].coor_D[0])) //shift point of stacker_No2's forearm on track stacker_No1
				{
					Stacker[Stacker_No1].shift[1] = 0;
					Stacker[Stacker_No2].shift[0] = 0;
				}
				else
				{
					Stacker[Stacker_No1].shift[0] = 0;
					Stacker[Stacker_No2].shift[1] = 0;
				}
			}
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 2) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 6))
		{
			vector_angle_calculation(Stacker, Stacker_No1, Stacker_No2, Slope2, b2, Stacker[Stacker_No1].coor_B[0], Stacker[Stacker_No1].coor_B[1]);
			if ((Stacker[Stacker_No1].Rotation_angle != 0) & (Stacker[Stacker_No2].Rotation_angle != 0))
			{
				if (shift_point_calculation(Stacker[Stacker_No2].Rotation_angle, b2, Stacker_No1, Stacker[Stacker_No1].coor_D[0], Stacker[Stacker_No2].coor_D[0])) //shift point of stacker_No2's forearm on track stacker_No1
				{
					Stacker[Stacker_No1].shift[1] = 0;
					Stacker[Stacker_No2].shift[0] = 0;
				}
				else
				{
					Stacker[Stacker_No1].shift[0] = 0;
					Stacker[Stacker_No2].shift[1] = 0;
				}
			}
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 3) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 6))
		{
			if (Stacker[Stacker_No2].Vertical_distance < Vertical_safety_distance)
			{
				vector_angle_calculation(Stacker, Stacker_No1, Stacker_No2, Slope2, b2, Stacker[Stacker_No1].coor_C[0], Stacker[Stacker_No1].coor_C[1]);
			}
			else
			{
				Stacker[Stacker_No2].Pitch[1] = 0;
			}
			if (shift_point_calculation(Stacker[Stacker_No2].Rotation_angle, b2, Stacker_No1, Stacker[Stacker_No1].coor_D[0], Stacker[Stacker_No2].coor_D[0])) //shift point of stacker_No2's forearm on track stacker_No1
			{
				Stacker[Stacker_No1].shift[1] = 0;
				Stacker[Stacker_No2].shift[0] = 0;
			}
			else
			{
				Stacker[Stacker_No1].shift[0] = 0;
				Stacker[Stacker_No2].shift[1] = 0;
			}
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 4) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 7))
		{
			if (Stacker_No2 == 1)
				b2 = -12;
			else
				b2 = 0;
			if (Stacker[Stacker_No1].Vertical_distance < Vertical_safety_distance)
				vector_angle_calculation(Stacker, Stacker_No1, Stacker_No2, 0, b2, Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_A[1]);
			else
				Stacker[Stacker_No1].Pitch[1] = 0;
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 5) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 7))
		{
			if (Stacker_No2 == 1)
				b2 = -12;
			else
				b2 = 0;
			if (Stacker[Stacker_No1].Vertical_distance < Vertical_safety_distance)
				vector_angle_calculation(Stacker, Stacker_No1, Stacker_No2, 0, b2, Stacker[Stacker_No1].coor_B[0], Stacker[Stacker_No1].coor_B[1]);
			else
				Stacker[Stacker_No1].Pitch[0] = 0;
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 6) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 1))
		{
			vector_angle_calculation(Stacker, Stacker_No2, Stacker_No1, Slope1, b1, Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_A[1]);
			if ((Stacker[Stacker_No1].Rotation_angle != 0) & (Stacker[Stacker_No2].Rotation_angle != 0))
			{
				if (shift_point_calculation(Stacker[Stacker_No1].Rotation_angle, b1, Stacker_No1, Stacker[Stacker_No2].coor_D[0], Stacker[Stacker_No1].coor_D[0])) //shift point of stacker_No2's forearm on track stacker_No1
				{
					Stacker[Stacker_No2].shift[1] = 0;
					Stacker[Stacker_No1].shift[0] = 0;
				}
				else
				{
					Stacker[Stacker_No2].shift[0] = 0;
					Stacker[Stacker_No1].shift[1] = 0;
				}
			}
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 6) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 2))
		{
			vector_angle_calculation(Stacker, Stacker_No2, Stacker_No1, Slope1, b1, Stacker[Stacker_No2].coor_B[0], Stacker[Stacker_No2].coor_B[1]);
			if ((Stacker[Stacker_No1].Rotation_angle != 0) & (Stacker[Stacker_No2].Rotation_angle != 0))
			{
				if (shift_point_calculation(Stacker[Stacker_No1].Rotation_angle, b1, Stacker_No1, Stacker[Stacker_No2].coor_D[0], Stacker[Stacker_No1].coor_D[0])) //shift point of stacker_No2's forearm on track stacker_No1
				{
					Stacker[Stacker_No2].shift[1] = 0;
					Stacker[Stacker_No1].shift[0] = 0;
				}
				else
				{
					Stacker[Stacker_No2].shift[0] = 0;
					Stacker[Stacker_No1].shift[1] = 0;
				}
			}
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 6) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 3))
		{
			if (Stacker[Stacker_No1].Vertical_distance < Vertical_safety_distance)
				vector_angle_calculation(Stacker, Stacker_No2, Stacker_No1, Slope1, b1, Stacker[Stacker_No2].coor_C[0], Stacker[Stacker_No2].coor_C[1]);
			else
				Stacker[Stacker_No1].Pitch[1] = 0;
			if (shift_point_calculation(Stacker[Stacker_No1].Rotation_angle, b1, Stacker_No2, Stacker[Stacker_No2].coor_D[0], Stacker[Stacker_No1].coor_D[0])) //shift point of stacker_No2's forearm on track stacker_No1
			{
				Stacker[Stacker_No2].shift[1] = 0;
				Stacker[Stacker_No1].shift[0] = 0;
			}
			else
			{
				Stacker[Stacker_No2].shift[0] = 0;
				Stacker[Stacker_No1].shift[1] = 0;
			}
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 7) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 4))
		{
			if (Stacker_No1 == 1)
				b1 = -12;
			else
				b1 = 0;
			if (Stacker[Stacker_No2].Vertical_distance < Vertical_safety_distance)
				vector_angle_calculation(Stacker, Stacker_No2, Stacker_No1, 0, b1, Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_A[1]);
			else
				Stacker[Stacker_No2].Pitch[1] = 0;
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 7) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 5))
		{
			if (Stacker_No1 == 1)
				b1 = -12;
			else
				b1 = 0;
			if (Stacker[Stacker_No2].Vertical_distance < Vertical_safety_distance)
				vector_angle_calculation(Stacker, Stacker_No2, Stacker_No1, 0, b1, Stacker[Stacker_No2].coor_B[0], Stacker[Stacker_No2].coor_B[1]);
			else
				Stacker[Stacker_No2].Pitch[0] = 0;
		}
		else if ((Stacker[Stacker_No1].collision_flag[Stacker_No2] == 8) & (Stacker[Stacker_No2].collision_flag[Stacker_No1] == 8))
		{
			if ((Stacker[Stacker_No1].Vertex[Stacker_No2] == 1) & (Stacker[Stacker_No2].Vertex[Stacker_No1] == 1))
			{
				if (Stacker[Stacker_No1].coor_A[1] > Stacker[Stacker_No2].coor_A[1])
				{
					Stacker[Stacker_No1].Rotation[0] = 0;
					Stacker[Stacker_No2].Rotation[1] = 0;
				}
				else
				{
					Stacker[Stacker_No1].Rotation[1] = 0;
					Stacker[Stacker_No2].Rotation[0] = 0;
				}
			}
			else if ((Stacker[Stacker_No1].Vertex[Stacker_No2] == 2) & (Stacker[Stacker_No2].Vertex[Stacker_No1] == 2))
			{
				if (Stacker[Stacker_No1].coor_B[1] > Stacker[Stacker_No2].coor_B[1])
				{
					Stacker[Stacker_No1].Rotation[1] = 0;
					Stacker[Stacker_No2].Rotation[0] = 0;
				}
				else
				{
					Stacker[Stacker_No1].Rotation[0] = 0;
					Stacker[Stacker_No2].Rotation[1] = 0;
				}
			}
			else if ((Stacker[Stacker_No1].Vertex[Stacker_No2] == 1) & (Stacker[Stacker_No2].Vertex[Stacker_No1] == 2))
			{
				Stacker[Stacker_No1].shift[0] = 0;
				Stacker[Stacker_No2].shift[1] = 0;
				Vertex_rotation_calculation(Stacker[Stacker_No1].coor_A[0], Stacker[Stacker_No1].coor_A[1], Stacker[Stacker_No1].coor_D[0], Stacker[Stacker_No1].coor_D[1], \
					Stacker[Stacker_No2].coor_B[0], Stacker[Stacker_No2].coor_B[1], Stacker[Stacker_No2].coor_D[0], Stacker[Stacker_No2].coor_D[0], Stacker, Stacker_No1, Stacker_No2);
			}
			else if ((Stacker[Stacker_No1].Vertex[Stacker_No2] == 2) & (Stacker[Stacker_No2].Vertex[Stacker_No1] == 1))
			{
				Stacker[Stacker_No2].shift[0] = 0;
				Stacker[Stacker_No1].shift[1] = 0;
				Vertex_rotation_calculation(Stacker[Stacker_No2].coor_A[0], Stacker[Stacker_No2].coor_A[1], Stacker[Stacker_No2].coor_D[0], Stacker[Stacker_No2].coor_D[1], \
					Stacker[Stacker_No1].coor_B[0], Stacker[Stacker_No1].coor_B[1], Stacker[Stacker_No1].coor_D[0], Stacker[Stacker_No1].coor_D[0], Stacker, Stacker_No2, Stacker_No1);
			}
			else if ((Stacker[Stacker_No1].Vertex[Stacker_No2] == 1) & (Stacker[Stacker_No2].Vertex[Stacker_No1] == 3))
			{
				Stacker[Stacker_No1].shift[0] = 0;
				Stacker[Stacker_No2].shift[1] = 0;
			}
			else if ((Stacker[Stacker_No1].Vertex[Stacker_No2] == 3) & (Stacker[Stacker_No2].Vertex[Stacker_No1] == 1))
			{
				Stacker[Stacker_No1].shift[1] = 0;
				Stacker[Stacker_No2].shift[0] = 0;
			}
			else
				printf("\n stacker_No1 %d vertex %d, stacker_No2 %d vertex %d", Stacker_No1, Stacker[Stacker_No1].Vertex[Stacker_No2], Stacker_No2, Stacker[Stacker_No2].Vertex[Stacker_No1]);
		}
		else
		{
			printf("\n Stacker %d collision %d = %d,Stacker %d collision %d = %d", Stacker_No1, Stacker_No2, Stacker[Stacker_No1].collision_flag[Stacker_No2], Stacker_No2, Stacker_No1, Stacker[Stacker_No2].collision_flag[Stacker_No1]);
		}
	}

}

void vector_angle_calculation(Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2, double k, double b, double X, double Y)
{
	double vector_collision[2] = { X - Stacker[Stacker_No2].coor_D[0],Y - Stacker[Stacker_No2].coor_D[1] };
	double vector_D1_collision[2] = { X - Stacker[Stacker_No1].coor_D[0], Y - Stacker[Stacker_No1].coor_D[1] };
	double vector_pedal[2];
	double vector_D1_pedal[2];
	double coor_pedal[2];
	if (fabs(Stacker[Stacker_No2].Rotation_angle) != 0.5 * PI)
	{
		pedal_coor_calculation(X, Y, k, b, coor_pedal);
	}
	else
	{
		coor_pedal[0] = 0;
		coor_pedal[1] = Y;
	}
	vector_pedal[0] = coor_pedal[0] - Stacker[Stacker_No2].coor_D[0];
	vector_pedal[1] = coor_pedal[1] - Stacker[Stacker_No2].coor_D[1];
	if (Stacker[Stacker_No2].collision_flag[Stacker_No1] != 7)
	{
		if (vector_pedal[0] * vector_collision[1] - vector_collision[0] * vector_pedal[1] > 0) // counterclockwise
			Stacker[Stacker_No2].Rotation[1] = 0;
		else
			Stacker[Stacker_No2].Rotation[0] = 0;
	}
	vector_D1_pedal[0] = coor_pedal[0] - Stacker[Stacker_No1].coor_D[0];
	vector_D1_pedal[1] = coor_pedal[1] - Stacker[Stacker_No1].coor_D[1];
	if (Stacker[Stacker_No1].collision_flag[Stacker_No2] != 3)
	{
		if (vector_D1_collision[0] * vector_D1_pedal[1] - vector_D1_collision[1] * vector_D1_pedal[0] > 0)   //
			Stacker[Stacker_No1].Rotation[1] = 0;
		else
			Stacker[Stacker_No1].Rotation[0] = 0;
	}
}

void Vertex_rotation_calculation(double X_1, double Y_1, double X_C_1, double Y_C_1, double X_2, double Y_2, double X_C_2, double Y_C_2, Stacker_reclaimer_t* Stacker, uint8 Stacker_No1, uint8 Stacker_No2)
{
	double vector_C1_P1[2] = { X_1 - X_C_1, Y_1 - Y_C_1 };
	double vector_C2_P2[2] = { X_2 - X_C_2, Y_2 - Y_C_2 };
	double vector_C1_P2[2] = { X_2 - X_C_1, Y_2 - Y_C_2 };
	double vector_C2_P1[2] = { X_1 - X_C_2, Y_1 - Y_C_1 };
	if (vector_C1_P1[0] * vector_C1_P2[1] - vector_C1_P1[1] * vector_C2_P2[0] > 0)
		Stacker[Stacker_No1].Rotation[1] = 0;
	else
		Stacker[Stacker_No1].Rotation[0] = 0;
	if (vector_C2_P2[0] * vector_C2_P1[1] - vector_C2_P2[1] * vector_C2_P1[0] > 0)
		Stacker[Stacker_No2].Rotation[1] = 0;
	else
		Stacker[Stacker_No2].Rotation[0] = 0;
}

void pedal_coor_calculation(double X, double Y, double k, double b, double* coor_pedal)
{
	double Numerator_X, Numerator_Y;
	double Denominator_X, Denominator_Y;
	Numerator_X = X - k * b + k * Y;
	Denominator_X = 1 + k * k;
	Numerator_Y = k * X + k * k * Y + b;
	Denominator_Y = 1 + k * k;
	coor_pedal[0] = Numerator_X / Denominator_X;
	coor_pedal[1] = Numerator_Y / Denominator_Y;
}

bool shift_point_calculation(double Rotation_angle, double b, uint8 Stacker_No1, double X_2, double X_1)	//return true: shift_point on the left;return false: shift_point on the right
{
	double slope;
	double shift_point;
	if (fabs(Rotation_angle) != 0.5 * PI)
		slope = tan(Rotation_angle);
	else
	{
		shift_point = X_1;
		if (shift_point < X_2)
			return true;
		else
			return false;
	}
	if (Rotation_angle != 0)
	{
		if (Stacker_No1 == 1)
			shift_point = (-12.0 - b) / slope;
		else
			shift_point = -b / slope;
	}
	else
	{
		shift_point = 1000;
	}
	if (shift_point < X_2)
		return true;
	else
		return false;
}